/* 
 * File:   TransformMatrix.cpp
 * Author: brady
 * 
 * Created on September 6, 2011, 6:20 PM
 */

#include <core/TransformMatrix.h>

TransformMatrix::TransformMatrix() {
    setIdentity();
}

TransformMatrix::~TransformMatrix() {
}

void TransformMatrix::setIdentity() {
    for (int i = 0; i < 16; i++) {
        Matrix[i] = ((i%5 == 0)? 1 : 0);
    }
}

void TransformMatrix::setZero()
{
    for (int i = 0; i < 16; i++) {
        Matrix[i] = 0;
    }
}

void TransformMatrix::rotate(double degrees, int axis)
{
    double sinDeg = -sin(degrees*(M_PI/180.0));
    double cosDeg = -cos(degrees*(M_PI/180.0));
    TransformMatrix temp;
    temp.setIdentity();
    double* m = temp.Matrix;
    switch (axis)
    {
        case X_AXIS:
        {
            m[5] = m[10] = cosDeg;
            m[6] = sinDeg * -1.0;
            m[9] = sinDeg;
            break;
        }

        case Y_AXIS:
        {
            m[0] = m[10] = cosDeg;
            m[2] = sinDeg;
            m[8] = sinDeg * -1.0;

            break;
        }

        case Z_AXIS:
        {
            m[0] = m[5] = cosDeg;
            m[1] = sinDeg * -1.0;
            m[4] = sinDeg;
            break;
        }
    }
    multiply(temp);
}

void TransformMatrix::translate(double dx, double dy, double dz)
{
    TransformMatrix temp;
    temp.setIdentity();
    temp.Matrix[R3+C0] = dx;
    temp.Matrix[R3+C1] = dy;
    temp.Matrix[R3+C2] = dz;
    multiply(temp);
}

void TransformMatrix::scale(double sx, double sy, double sz)
{

//    Matrix[R0+C0] *= sx;
//    Matrix[R1+C1] *= sy;
//    Matrix[R2+C2] *= sz;

    TransformMatrix temp;
    temp.setIdentity();
    temp.Matrix[R0+C0] = sx;
    temp.Matrix[R1+C1] = sy;
    temp.Matrix[R2+C2] = sz;
    multiply(temp);
}

void TransformMatrix::multiply(TransformMatrix& m)
{
    double* l = Matrix;
    double* r = m.Matrix;
    double newMatrix[16];
    for(int i = 0; i<4; i++){
        for(int j=0; j<4; j++){
          newMatrix[j + 4*i] = l[0 +4*i]*r[0 + j] + l[1 +4*i]*r[4 + j] + l[2 +4*i]*r[8 + j] + l[3 +4*i]*r[12 + j];
        }
    }
    for(int i = 0; i<16; i++)
    {
        Matrix[i] = newMatrix[i];
    }
}

Vector3D TransformMatrix::transform(const Vector3D& vector)
{
    double x, y, z;
    double x0 = vector[0];
    double y0 = vector[1];
    double z0 = vector[2];
    x = x0 * Matrix[R0+C0] + y0 * Matrix[R1+C0] + z0 * Matrix[R2+C0] +
            Matrix[R3+C0];

    y = x0 * Matrix[R0+C1] + y0 * Matrix[R1+C1] + z0 * Matrix[R2+C1] +
            Matrix[R3+C1];

    z = x0 * Matrix[R0+C2] + y0 * Matrix[R1+C2] + z0 * Matrix[R2+C2] +
            Matrix[R3+C2];

    return Vector3D(x, y, z);
}

Vector3D TransformMatrix::transposeTransform(const Vector3D& vector)
{
    double x, y, z;
    double x0 = vector[0];
    double y0 = vector[1];
    double z0 = vector[2];
    x = x0 * Matrix[R0+C0] + y0 * Matrix[R0+C1] + z0 * Matrix[R0+C2];
    y = x0 * Matrix[R1+C0] + y0 * Matrix[R1+C1] + z0 * Matrix[R1+C2];
    z = x0 * Matrix[R2+C0] + y0 * Matrix[R2+C1] + z0 * Matrix[R2+C2];
    return Vector3D(x, y, z);
}

/**
 * This call is EXPENSIVE! Save the result before using. DON'T compute repeatedly.
 * @return
 */
TransformMatrix TransformMatrix::getInverse()
{
    double InverseMatrix[16];
    TransformMatrix Inverse;
    double determinate;
    determinate =
       Matrix[R0+C3]*Matrix[R1+C2]*Matrix[R2+C1]*Matrix[R3+C0] - Matrix[R0+C2]*Matrix[R1+C3]*Matrix[R2+C1]*Matrix[R3+C0] - Matrix[R0+C3]*Matrix[R1+C1]*Matrix[R2+C2]*Matrix[R3+C0] + Matrix[R0+C1]*Matrix[R1+C3]*Matrix[R2+C2]*Matrix[R3+C0]+
       Matrix[R0+C2]*Matrix[R1+C1]*Matrix[R2+C3]*Matrix[R3+C0] - Matrix[R0+C1]*Matrix[R1+C2]*Matrix[R2+C3]*Matrix[R3+C0] - Matrix[R0+C3]*Matrix[R1+C2]*Matrix[R2+C0]*Matrix[R3+C1] + Matrix[R0+C2]*Matrix[R1+C3]*Matrix[R2+C0]*Matrix[R3+C1]+
       Matrix[R0+C3]*Matrix[R1+C0]*Matrix[R2+C2]*Matrix[R3+C1] - Matrix[R0+C0]*Matrix[R1+C3]*Matrix[R2+C2]*Matrix[R3+C1] - Matrix[R0+C2]*Matrix[R1+C0]*Matrix[R2+C3]*Matrix[R3+C1] + Matrix[R0+C0]*Matrix[R1+C2]*Matrix[R2+C3]*Matrix[R3+C1]+
       Matrix[R0+C3]*Matrix[R1+C1]*Matrix[R2+C0]*Matrix[R3+C2] - Matrix[R0+C1]*Matrix[R1+C3]*Matrix[R2+C0]*Matrix[R3+C2] - Matrix[R0+C3]*Matrix[R1+C0]*Matrix[R2+C1]*Matrix[R3+C2] + Matrix[R0+C0]*Matrix[R1+C3]*Matrix[R2+C1]*Matrix[R3+C2]+
       Matrix[R0+C1]*Matrix[R1+C0]*Matrix[R2+C3]*Matrix[R3+C2] - Matrix[R0+C0]*Matrix[R1+C1]*Matrix[R2+C3]*Matrix[R3+C2] - Matrix[R0+C2]*Matrix[R1+C1]*Matrix[R2+C0]*Matrix[R3+C3] + Matrix[R0+C1]*Matrix[R1+C2]*Matrix[R2+C0]*Matrix[R3+C3]+
       Matrix[R0+C2]*Matrix[R1+C0]*Matrix[R2+C1]*Matrix[R3+C3] - Matrix[R0+C0]*Matrix[R1+C2]*Matrix[R2+C1]*Matrix[R3+C3] - Matrix[R0+C1]*Matrix[R1+C0]*Matrix[R2+C2]*Matrix[R3+C3] + Matrix[R0+C0]*Matrix[R1+C1]*Matrix[R2+C2]*Matrix[R3+C3];

   InverseMatrix[R0+C0] = Matrix[R1+C2]*Matrix[R2+C3]*Matrix[R3+C1] - Matrix[R1+C3]*Matrix[R2+C2]*Matrix[R3+C1] + Matrix[R1+C3]*Matrix[R2+C1]*Matrix[R3+C2] - Matrix[R1+C1]*Matrix[R2+C3]*Matrix[R3+C2] - Matrix[R1+C2]*Matrix[R2+C1]*Matrix[R3+C3] + Matrix[R1+C1]*Matrix[R2+C2]*Matrix[R3+C3];
   InverseMatrix[R0+C1] = Matrix[R0+C3]*Matrix[R2+C2]*Matrix[R3+C1] - Matrix[R0+C2]*Matrix[R2+C3]*Matrix[R3+C1] - Matrix[R0+C3]*Matrix[R2+C1]*Matrix[R3+C2] + Matrix[R0+C1]*Matrix[R2+C3]*Matrix[R3+C2] + Matrix[R0+C2]*Matrix[R2+C1]*Matrix[R3+C3] - Matrix[R0+C1]*Matrix[R2+C2]*Matrix[R3+C3];
   InverseMatrix[R0+C2] = Matrix[R0+C2]*Matrix[R1+C3]*Matrix[R3+C1] - Matrix[R0+C3]*Matrix[R1+C2]*Matrix[R3+C1] + Matrix[R0+C3]*Matrix[R1+C1]*Matrix[R3+C2] - Matrix[R0+C1]*Matrix[R1+C3]*Matrix[R3+C2] - Matrix[R0+C2]*Matrix[R1+C1]*Matrix[R3+C3] + Matrix[R0+C1]*Matrix[R1+C2]*Matrix[R3+C3];
   InverseMatrix[R0+C3] = Matrix[R0+C3]*Matrix[R1+C2]*Matrix[R2+C1] - Matrix[R0+C2]*Matrix[R1+C3]*Matrix[R2+C1] - Matrix[R0+C3]*Matrix[R1+C1]*Matrix[R2+C2] + Matrix[R0+C1]*Matrix[R1+C3]*Matrix[R2+C2] + Matrix[R0+C2]*Matrix[R1+C1]*Matrix[R2+C3] - Matrix[R0+C1]*Matrix[R1+C2]*Matrix[R2+C3];
   InverseMatrix[R1+C0] = Matrix[R1+C3]*Matrix[R2+C2]*Matrix[R3+C0] - Matrix[R1+C2]*Matrix[R2+C3]*Matrix[R3+C0] - Matrix[R1+C3]*Matrix[R2+C0]*Matrix[R3+C2] + Matrix[R1+C0]*Matrix[R2+C3]*Matrix[R3+C2] + Matrix[R1+C2]*Matrix[R2+C0]*Matrix[R3+C3] - Matrix[R1+C0]*Matrix[R2+C2]*Matrix[R3+C3];
   InverseMatrix[R1+C1] = Matrix[R0+C2]*Matrix[R2+C3]*Matrix[R3+C0] - Matrix[R0+C3]*Matrix[R2+C2]*Matrix[R3+C0] + Matrix[R0+C3]*Matrix[R2+C0]*Matrix[R3+C2] - Matrix[R0+C0]*Matrix[R2+C3]*Matrix[R3+C2] - Matrix[R0+C2]*Matrix[R2+C0]*Matrix[R3+C3] + Matrix[R0+C0]*Matrix[R2+C2]*Matrix[R3+C3];
   InverseMatrix[R1+C2] = Matrix[R0+C3]*Matrix[R1+C2]*Matrix[R3+C0] - Matrix[R0+C2]*Matrix[R1+C3]*Matrix[R3+C0] - Matrix[R0+C3]*Matrix[R1+C0]*Matrix[R3+C2] + Matrix[R0+C0]*Matrix[R1+C3]*Matrix[R3+C2] + Matrix[R0+C2]*Matrix[R1+C0]*Matrix[R3+C3] - Matrix[R0+C0]*Matrix[R1+C2]*Matrix[R3+C3];
   InverseMatrix[R1+C3] = Matrix[R0+C2]*Matrix[R1+C3]*Matrix[R2+C0] - Matrix[R0+C3]*Matrix[R1+C2]*Matrix[R2+C0] + Matrix[R0+C3]*Matrix[R1+C0]*Matrix[R2+C2] - Matrix[R0+C0]*Matrix[R1+C3]*Matrix[R2+C2] - Matrix[R0+C2]*Matrix[R1+C0]*Matrix[R2+C3] + Matrix[R0+C0]*Matrix[R1+C2]*Matrix[R2+C3];
   InverseMatrix[R2+C0] = Matrix[R1+C1]*Matrix[R2+C3]*Matrix[R3+C0] - Matrix[R1+C3]*Matrix[R2+C1]*Matrix[R3+C0] + Matrix[R1+C3]*Matrix[R2+C0]*Matrix[R3+C1] - Matrix[R1+C0]*Matrix[R2+C3]*Matrix[R3+C1] - Matrix[R1+C1]*Matrix[R2+C0]*Matrix[R3+C3] + Matrix[R1+C0]*Matrix[R2+C1]*Matrix[R3+C3];
   InverseMatrix[R2+C1] = Matrix[R0+C3]*Matrix[R2+C1]*Matrix[R3+C0] - Matrix[R0+C1]*Matrix[R2+C3]*Matrix[R3+C0] - Matrix[R0+C3]*Matrix[R2+C0]*Matrix[R3+C1] + Matrix[R0+C0]*Matrix[R2+C3]*Matrix[R3+C1] + Matrix[R0+C1]*Matrix[R2+C0]*Matrix[R3+C3] - Matrix[R0+C0]*Matrix[R2+C1]*Matrix[R3+C3];
   InverseMatrix[R2+C2] = Matrix[R0+C1]*Matrix[R1+C3]*Matrix[R3+C0] - Matrix[R0+C3]*Matrix[R1+C1]*Matrix[R3+C0] + Matrix[R0+C3]*Matrix[R1+C0]*Matrix[R3+C1] - Matrix[R0+C0]*Matrix[R1+C3]*Matrix[R3+C1] - Matrix[R0+C1]*Matrix[R1+C0]*Matrix[R3+C3] + Matrix[R0+C0]*Matrix[R1+C1]*Matrix[R3+C3];
   InverseMatrix[R2+C3] = Matrix[R0+C3]*Matrix[R1+C1]*Matrix[R2+C0] - Matrix[R0+C1]*Matrix[R1+C3]*Matrix[R2+C0] - Matrix[R0+C3]*Matrix[R1+C0]*Matrix[R2+C1] + Matrix[R0+C0]*Matrix[R1+C3]*Matrix[R2+C1] + Matrix[R0+C1]*Matrix[R1+C0]*Matrix[R2+C3] - Matrix[R0+C0]*Matrix[R1+C1]*Matrix[R2+C3];
   InverseMatrix[R3+C0] = Matrix[R1+C2]*Matrix[R2+C1]*Matrix[R3+C0] - Matrix[R1+C1]*Matrix[R2+C2]*Matrix[R3+C0] - Matrix[R1+C2]*Matrix[R2+C0]*Matrix[R3+C1] + Matrix[R1+C0]*Matrix[R2+C2]*Matrix[R3+C1] + Matrix[R1+C1]*Matrix[R2+C0]*Matrix[R3+C2] - Matrix[R1+C0]*Matrix[R2+C1]*Matrix[R3+C2];
   InverseMatrix[R3+C1] = Matrix[R0+C1]*Matrix[R2+C2]*Matrix[R3+C0] - Matrix[R0+C2]*Matrix[R2+C1]*Matrix[R3+C0] + Matrix[R0+C2]*Matrix[R2+C0]*Matrix[R3+C1] - Matrix[R0+C0]*Matrix[R2+C2]*Matrix[R3+C1] - Matrix[R0+C1]*Matrix[R2+C0]*Matrix[R3+C2] + Matrix[R0+C0]*Matrix[R2+C1]*Matrix[R3+C2];
   InverseMatrix[R3+C2] = Matrix[R0+C2]*Matrix[R1+C1]*Matrix[R3+C0] - Matrix[R0+C1]*Matrix[R1+C2]*Matrix[R3+C0] - Matrix[R0+C2]*Matrix[R1+C0]*Matrix[R3+C1] + Matrix[R0+C0]*Matrix[R1+C2]*Matrix[R3+C1] + Matrix[R0+C1]*Matrix[R1+C0]*Matrix[R3+C2] - Matrix[R0+C0]*Matrix[R1+C1]*Matrix[R3+C2];
   InverseMatrix[R3+C3] = Matrix[R0+C1]*Matrix[R1+C2]*Matrix[R2+C0] - Matrix[R0+C2]*Matrix[R1+C1]*Matrix[R2+C0] + Matrix[R0+C2]*Matrix[R1+C0]*Matrix[R2+C1] - Matrix[R0+C0]*Matrix[R1+C2]*Matrix[R2+C1] - Matrix[R0+C1]*Matrix[R1+C0]*Matrix[R2+C2] + Matrix[R0+C0]*Matrix[R1+C1]*Matrix[R2+C2];

   for(int i = 0 ; i<16; i++)
   {
       InverseMatrix[i] = InverseMatrix[i]*(1.0/determinate);
       Inverse.Matrix[i] = InverseMatrix[i];
   }
   return Inverse;
}

void TransformMatrix::verifyInverse()
{
    TransformMatrix temp = this->getInverse();
    temp.multiply(*this);
    for(int i = 0; i<16; i++)
    {
        if(i%4 == 0) cout << "\n";
        cout << "[" << temp.Matrix[i] <<  "]";
    }
    cout << endl;
}